package rov.math;


// simple variable-sized matrix class
public class Matrix
{
    private static final int X = 0;
    private static final int Y = 1;
    private static final int Z = 2;
    private static final int W = 3;
    public double[][] mat;


    // generic arbitrary size constructor
    private Matrix(int rows, int cols)
    {
        mat = new double[rows][cols];
    }


    // 1 row by 4 column constructor (for Quaternion, Axis-Angle, etc)
    public Matrix(double x, double y, double z, double w)
    {
        this(1, 4);

        mat[0][X] = x;
        mat[0][Y] = y;
        mat[0][Z] = z;
        mat[0][W] = w;
    }


    // 1 row by 3 column constructor (for Vector3, etc)
    public Matrix(double x, double y, double z)
    {
        this(1, 3);

        mat[0][X] = x;
        mat[0][Y] = y;
        mat[0][Z] = z;
    }


    public Matrix(double x0, double y0, double z0, double w0,
                  double x1, double y1, double z1, double w1,
                  double x2, double y2, double z2, double w2,
                  double x3, double y3, double z3, double w3)
    {
        this(4, 4);

        mat[0][X] = x0;
        mat[0][Y] = y0;
        mat[0][Z] = z0;
        mat[0][W] = w0;

        mat[1][X] = x1;
        mat[1][Y] = y1;
        mat[1][Z] = z1;
        mat[1][W] = w1;

        mat[2][X] = x2;
        mat[2][Y] = y2;
        mat[2][Z] = z2;
        mat[2][W] = w2;

        mat[3][X] = x3;
        mat[3][Y] = y3;
        mat[3][Z] = z3;
        mat[3][W] = w3;
    }


    // returns the multiplication of 2 quaternions
    public Matrix multiplyQuats(Matrix m)
    {
        //if (mat[0].length != m.mat.length || mat.length != m.mat[0].length)
        //    throw new IllegalArgumentException("Matricies' sizes are incompatible for multiplication");

        if (mat[0].length != 4 || mat.length != 1 || m.mat.length != 1 || m.mat[0].length != 4)
            throw new IllegalArgumentException("Both matricies must be quaternions");

        double x = mat[0][W] * m.mat[0][X] + mat[0][X] * m.mat[0][W] + mat[0][Y] * m.mat[0][Z] - mat[0][Z] * m.mat[0][Y];
        double y = mat[0][W] * m.mat[0][Y] + mat[0][Y] * m.mat[0][W] + mat[0][Z] * m.mat[0][X] - mat[0][X] * m.mat[0][Z];
        double z = mat[0][W] * m.mat[0][Z] + mat[0][Z] * m.mat[0][W] + mat[0][X] * m.mat[0][Y] - mat[0][Y] * m.mat[0][X];
        double w = mat[0][W] * m.mat[0][W] - mat[0][X] * m.mat[0][X] - mat[0][Y] * m.mat[0][Y] - mat[0][Z] * m.mat[0][Z];

        return new Matrix(x, y, z, w);
    }


    // returns the magnitude of a single row in the matrix
    public double getMagnitude(int row)
    {
        double len = 0;

        for (int i = 0; i < mat[row].length; i++)
            len += mat[row][i] * mat[row][i];

        return (double) Math.sqrt(len);
    }


    // normalizes a single row in the matrix
    public void normalizeInplace(int row)
    {
        double mag = getMagnitude(row);

        for (int i = 0; i < mat[row].length; i++)
            mat[row][i] /= mag;
    }


    // returns a new matrix (Quaternion) from the current matrix (Axis-Angle)
    public Matrix axisAngleToQuat()
    {
        if (mat.length != 1 || mat[0].length != 4)
            throw new IllegalArgumentException("Matrix is not a quaternion");

        double angle = mat[0][W] * 0.5f;
        Matrix v = new Matrix(mat[0][X], mat[0][Y], mat[0][Z]);

        v.normalizeInplace(0);

        double sinAngle = Math.sin(angle);
        return new Matrix(v.mat[0][X] * sinAngle, v.mat[0][Y] * sinAngle,
                          v.mat[0][Z] * sinAngle, Math.cos(angle));
    }


    public Matrix quatToAxisAngle()
    {
        double mag = getMagnitude(0);

        return new Matrix(mat[0][X] / mag, mat[0][Y] / mag, mat[0][Z] / mag,
                          Math.acos(mat[0][W]) * 2.0f);
    }


    public Matrix quatToMatrix()
    {
	double x2 = mat[0][X] * mat[0][X];
	double y2 = mat[0][Y] * mat[0][Y];
	double z2 = mat[0][Z] * mat[0][Z];

	double xy = mat[0][X] * mat[0][Y];
	double xz = mat[0][X] * mat[0][Z];
	double yz = mat[0][Y] * mat[0][Z];

	double wx = mat[0][W] * mat[0][X];
	double wy = mat[0][W] * mat[0][Y];
	double wz = mat[0][W] * mat[0][Z];

	return new Matrix(	1.0f - 2.0f * (y2 + z2), 2.0f * (xy - wz), 2.0f * (xz + wy), 0.0f,
				2.0f * (xy + wz), 1.0f - 2.0f * (x2 + z2), 2.0f * (yz - wx), 0.0f,
				2.0f * (xz - wy), 2.0f * (yz + wx), 1.0f - 2.0f * (x2 + y2), 0.0f,
				0.0f, 0.0f, 0.0f, 1.0f);
    }


    public void print()
    {
        /*for (int y = 0; y < mat.length; y++)
        {
            System.out.print(y + ") ");

            for (int x = 0; x < mat[0].length; x++)
                System.out.print(mat[y][x]/Math.PI*180.0 + "\t");

            System.out.println();
        }*/

        if (mat[0].length == 4)
            System.out.println((int) (mat[0][3]/Math.PI*180.0 * 100) / 100.0);
    }
}
